#include "stm32f10x.h"                  // Device header
#include "Servo.h"
#include "Delay.h"

// 定义两个舵机的参数（修正PWM范围：均为500-2500，对应0.5ms-2.5ms）
#define SERVO1_ANGLE_MIN 0.0f     // PA1-270度舵机：最小角度（0度）
#define SERVO1_ANGLE_MAX 270.0f   // PA1-270度舵机：最大角度（270度）
#define SERVO1_PWM_MIN 500        // PA1舵机：最小PWM值（对应0.5ms脉宽）
#define SERVO1_PWM_MAX 2500       // PA1舵机：最大PWM值（对应2.5ms脉宽）

#define SERVO2_ANGLE_MIN 0.0f     // PA2-180度舵机：最小角度（0度）
#define SERVO2_ANGLE_MAX 180.0f   // PA2-180度舵机：最大角度（180度）
#define SERVO2_PWM_MIN 500        // PA2舵机：最小PWM值（对应0.5ms脉宽）
#define SERVO2_PWM_MAX 2500       // PA2舵机：最大PWM值（对应2.5ms脉宽）

// 外部声明PID控制器及状态变量（保持原逻辑）
extern PID_Controller panPID;        // 水平轴（270度舵机，PA1）PID控制器
extern PID_Controller tiltPID;       // 垂直轴（180度舵机，PA2）PID控制器
extern float servo_rotation_value;   // PA1当前角度（0~270度）
extern float servo_pitch_value;      // PA2当前角度（0~180度）
extern uint8_t servo_rotation_direction;  // 水平方向反转标志（0：反转，1：正转）
extern uint8_t servo_pitch_direction;     // 垂直方向反转标志（0：反转，1：正转）


/**
  * 函    数：舵机初始化
  * 功    能：初始化舵机控制所需的PWM输出（TIM2定时器，PA1/PA2引脚）
  * 说    明：底层PWM配置为20ms周期（舵机标准周期），输出通道对应PA1（TIM2_CH1）和PA2（TIM2_CH2）
  */
void Servo_Init(void)
{
    PWM_Init();  // 初始化底层PWM驱动（需确保PWM_Init配置正确：20ms周期，500-2500的PWM值有效）
}


/**
  * 函    数：设置PA1控制的270度舵机角度
  * 参    数：Angle - 目标角度（范围：0.0~270.0度，超出范围会自动限制到边界）
  * 映射关系：角度与PWM线性映射（0度→500，270度→2500）
  * 脉宽说明：500对应0.5ms脉宽，2500对应2.5ms脉宽（PWM计数器时钟需匹配此比例）
  */
void Servo_SetAngle1(float Angle)
{
    // 限制角度在有效范围（0~270度）
    if (Angle > SERVO1_ANGLE_MAX)
    {
        Angle = SERVO1_ANGLE_MAX;
    }
    if (Angle < SERVO1_ANGLE_MIN)
    {
        Angle = SERVO1_ANGLE_MIN;
    }
    
    // 角度转PWM计算：线性映射公式
    // PWM = 最小PWM + (角度 / 最大角度) * (最大PWM - 最小PWM)
    uint16_t PWM = SERVO1_PWM_MIN + (uint32_t)(SERVO1_PWM_MAX - SERVO1_PWM_MIN) * Angle / SERVO1_ANGLE_MAX;
    
    PWM_SetCompare1(PWM);  // 通过TIM2_CH1（PA1）输出计算得到的PWM值
}


/**
  * 函    数：设置PA2控制的180度舵机角度
  * 参    数：Angle - 目标角度（范围：0.0~180.0度，超出范围会自动限制到边界）
  * 映射关系：角度与PWM线性映射（0度→500，180度→2500）
  * 脉宽说明：同270度舵机，500对应0.5ms，2500对应2.5ms（确保与舵机硬件参数匹配）
  */
void Servo_SetAngle2(float Angle)
{
    // 限制角度在有效范围（0~180度）
    if (Angle > SERVO2_ANGLE_MAX)
    {
        Angle = SERVO2_ANGLE_MAX;
    }
    if (Angle < SERVO2_ANGLE_MIN)
    {
        Angle = SERVO2_ANGLE_MIN;
    }
    
    // 角度转PWM计算：线性映射公式
    uint16_t PWM = SERVO2_PWM_MIN + (uint32_t)(SERVO2_PWM_MAX - SERVO2_PWM_MIN) * Angle / SERVO2_ANGLE_MAX;
    
    PWM_SetCompare2(PWM);  // 通过TIM2_CH2（PA2）输出计算得到的PWM值
}


/**
  * 函    数：读取PA1控制的270度舵机当前角度
  * 返 回 值：当前角度（范围：0.0~270.0度）
  * 原    理：读取当前PWM比较值，反向计算对应的角度（基于PWM与角度的映射关系）
  */
float ReadServoAngle1(void)
{
    uint16_t pwmValue = PWM_GetCompare1();  // 读取TIM2_CH1当前PWM比较值（PA1输出的PWM）
    
    // PWM转角度计算：反向映射公式
    // 角度 = (PWM值 - 最小PWM) / (最大PWM - 最小PWM) * 最大角度
    float angle = (float)(pwmValue - SERVO1_PWM_MIN) / (SERVO1_PWM_MAX - SERVO1_PWM_MIN) * SERVO1_ANGLE_MAX;
    
    // 限制角度在有效范围（避免PWM异常时计算值超界）
    if (angle > SERVO1_ANGLE_MAX)
    {
        angle = SERVO1_ANGLE_MAX;
    }
    if (angle < SERVO1_ANGLE_MIN)
    {
        angle = SERVO1_ANGLE_MIN;
    }
    
    return angle;
}


/**
  * 函    数：读取PA2控制的180度舵机当前角度
  * 返 回 值：当前角度（范围：0.0~180.0度）
  * 原    理：读取当前PWM比较值，反向计算对应的角度（基于PWM与角度的映射关系）
  */
float ReadServoAngle2(void)
{
    uint16_t pwmValue = PWM_GetCompare2();  // 读取TIM2_CH2当前PWM比较值（PA2输出的PWM）
    
    // PWM转角度计算：反向映射公式
    float angle = (float)(pwmValue - SERVO2_PWM_MIN) / (SERVO2_PWM_MAX - SERVO2_PWM_MIN) * SERVO2_ANGLE_MAX;
    
    // 限制角度在有效范围
    if (angle > SERVO2_ANGLE_MAX)
    {
        angle = SERVO2_ANGLE_MAX;
    }
    if (angle < SERVO2_ANGLE_MIN)
    {
        angle = SERVO2_ANGLE_MIN;
    }
    
    return angle;
}


/**
  * 函    数：增量式调整舵机角度
  * 参    数：x - 水平方向角度增量（正/负值对应顺/逆时针）
  *           y - 垂直方向角度增量（正/负值对应上/下）
  * 功    能：根据增量值微调舵机角度，带角度限位保护
  * 说    明：通过方向反转标志可调整x、y的实际运动方向
  */
void camera_move_increment(float x, float y)
{
    // 水平方向反转处理（根据标志调整增量方向）
    if (servo_rotation_direction == 0)
    {
        x = -x;  // 反转水平方向
    }
    
    // 垂直方向反转处理
    if (servo_pitch_direction == 0)
    {
        y = -y;  // 反转垂直方向
    }
    
    // 水平舵机（270度）角度调整与限位
    // 仅当增量后的角度在有效范围（0~270度）内时执行调整
    if ((servo_rotation_value + x) > SERVO1_ANGLE_MIN && (servo_rotation_value + x) < SERVO1_ANGLE_MAX)
    {
        servo_rotation_value += x;  // 更新当前角度值
        Servo_SetAngle1(servo_rotation_value);  // 应用角度调整
    }
    
    // 垂直舵机（180度）角度调整与限位
    // 仅当增量后的角度在有效范围（0~180度）内时执行调整
    if ((servo_pitch_value + y) > SERVO2_ANGLE_MIN && (servo_pitch_value + y) < SERVO2_ANGLE_MAX)
    {
        servo_pitch_value += y;  // 更新当前角度值
        Servo_SetAngle2(servo_pitch_value);  // 应用角度调整
    }
    
    Delay_ms(10);  // 角度调整后延时，保证舵机运动稳定
}


/**
  * 函    数：闭环控制舵机对准目标
  * 参    数：target_x - 目标X坐标（如图像中心X）
  *           target_y - 目标Y坐标（如图像中心Y）
  *           laser_x  - 当前激光点X坐标（实际检测到的位置）
  *           laser_y  - 当前激光点Y坐标（实际检测到的位置）
  * 功    能：根据目标与当前位置的偏差，通过增量式调整实现闭环对准
  * 说    明：当前使用比例控制（系数0.01），可根据实际需求调整系数或启用PID控制
  */
void camera_move_to_target_close_loop(int target_x, int target_y, int laser_x, int laser_y)
{
    // 计算X、Y方向的偏差，乘以比例系数得到角度增量
    // 系数0.01可根据实际控制灵敏度调整（需通过调试优化）
//    float delta_x = (float)(target_x - laser_x) * 0.01f;
//    float delta_y = (float)(target_y - laser_y) * 0.01f;
//    
//    // 调用增量调整函数执行闭环控制
//    camera_move_increment(delta_x, delta_y);
    
    // 若启用PID控制，可替换为以下代码（需确保PID参数适配舵机特性）
    float delta_x = (float)PID_Compute(&panPID, target_x - laser_x);
    float delta_y = (float)PID_Compute(&tiltPID, target_y - laser_y);
    camera_move_increment(delta_x, delta_y);
}
